/*
 * mod_pid.c
 *
 *  Created on: 2025 Feb 18
 *      Author: SFLY
 */
#include "mod_pid.h"
#include "bsp_board_config.h"
/**
 ******************************************************************************
 ** \brief initialize incremental PI regulator
 **
 ** \param [in] stc_pid_cfg_t stcCfg: configuration parameters
 **             stc_inc_pid_t* pstc: pointer to PID regulator
 **
 ** \retval     none
 ******************************************************************************/
void PID_InitIncPid(float Kp,float Ki,float OutMin,float OutMax,float ErrMax, stc_inc_pid_t *pstc) {
    pstc->Kp = Kp;
    pstc->Ki = __builtin_dsp_fsdiv_qx320f(Ki, MOTOR_SYS_PWM_SWITCHING_FREQ_HZ);
    pstc->OutMin = OutMin;
    pstc->OutMax = OutMax;
    pstc->AbsErrMax = ErrMax;
    pstc->Output = 0;
    pstc->Err1 = 0;
}

/**
 ******************************************************************************
 ** \brief incremental PI regulator
 **
 ** \param [in] float Ref:  reference
 **             float Fb:   feedback
 **             stc_inc_pid_t* pstc: pointer to PID regulator
 **
 ** \retval     float Output
 ******************************************************************************/
float PID_IncPid(float Ref, float Fb, stc_inc_pid_t *pstc) {
    float Err = Ref - Fb;

    /** limit maximum error ***************************************************/
    if (Err > pstc->AbsErrMax) {
        Err = pstc->AbsErrMax;
    } else if (Err < -pstc->AbsErrMax) {
        Err = -pstc->AbsErrMax;
    }

    /** PI regulation considering round-off error of I-regulator **************/
    pstc->Output += pstc->Kp * (Err - pstc->Err1);              // Kp term
    pstc->Output += pstc->Ki * Err;                             // multiply higher bits of Ki

    if (pstc->Output > pstc->OutMax) {
        pstc->Output = pstc->OutMax;
    } else if (pstc->Output < pstc->OutMin) {
        pstc->Output = pstc->OutMin;
    }

    pstc->Err1 = Err;
    return pstc->Output;
}

///**
// ******************************************************************************
// ** \brief initialize position PI regulator
// **
// ** \param [in] stc_pid_cfg_t stcCfg: configuration parameters
// **             stc_inc_pid_t* pstc: pointer to PID regulator
// **
// ** \retval     none
// ******************************************************************************/
//void PID_InitPosPid(float Kp,float Ki,float OutMin,float OutMax,float ErrMax, stc_pos_pid_t *pstc) {
//    pstc->Kp = Kp;
//    pstc->Ki = Ki;
//    pstc->OutMin = OutMin;
//    pstc->OutMax = OutMax;
//    pstc->AbsErrMax = ErrMax;
//    pstc->Output = 0;
//    pstc->Iout = pstc->Output;
//    pstc->Err1 = 0;
//}
//
///**
// ******************************************************************************
// ** \brief position PI regulator
// **
// ** \param [in] float Ref:  reference
// **             float Fb:   feedback
// **             stc_inc_pid_t* pstc: pointer to PID regulator
// **
// ** \retval     float Output
// ******************************************************************************/
//float PID_PosPid(float Ref, float Fb, stc_pos_pid_t *pstc) {
//    float Err = Ref - Fb;
//    /** limit maximum error ***************************************************/
//    if (Err > pstc->AbsErrMax) {
//        Err = pstc->AbsErrMax;
//    } else if (Err < -pstc->AbsErrMax) {
//        Err = -pstc->AbsErrMax;
//    }
//    if (0 == pstc->Ki) {
//        pstc->Iout = 0;
//    } else {
//        /** I-term regulation considering round-off error of I-regulator **********/
//        pstc->Iout += pstc->Ki * Err;  // multiply higher bits of Ki
//
//        /** I-regulator output limitation *****************************************/
//        if (pstc->Iout > pstc->OutMax) {
//            pstc->Iout = pstc->OutMax;
//        } else if (pstc->Iout < pstc->OutMin) {
//            pstc->Iout = pstc->OutMin;
//        }
//    }
//
//    /** sum of P and I regulator **********************************************/
//    pstc->Output = pstc->Kp * Err + pstc->Iout;
//
//    /** output limitation *****************************************************/
//    if (pstc->Output > pstc->OutMax) {
//        pstc->Output = pstc->OutMax;
//    } else if (pstc->Output < pstc->OutMin) {
//        pstc->Output = pstc->OutMin;
//    }
//
//    pstc->Err1 = Err;
//    return pstc->Output;
//}

/**
 ******************************************************************************
 ** \brief initialize position PID regulator
 **
 ** \param [in] float Kp: 比例系数
 **             float Ki: 积分系数
 **             float Kd: 微分系数
 **             float OutMin: 输出最小值
 **             float OutMax: 输出最大值
 **             float ErrMax: 最大误差值
 **             float ErrSeparation: 积分分离阈值
 **             stc_pos_pid_t* pstc: pointer to PID regulator
 **
 ** \retval     none
 ******************************************************************************/
void PID_InitPosPid(float Kp, float Ki, float Kd, float OutMin, float OutMax, float ErrMax, float ErrSeparation, stc_pos_pid_t *pstc) {
    pstc->Kp = Kp;
    pstc->Ki = Ki;
    pstc->Kd = Kd;
    pstc->OutMin = OutMin;
    pstc->OutMax = OutMax;
    pstc->AbsErrMax = ErrMax;
    pstc->ErrSeparation = ErrSeparation;
    pstc->Output = 0;
    pstc->Iout = pstc->Output;
    pstc->Err1 = 0;
    pstc->Err2 = 0;
}

/**
 ******************************************************************************
 ** \brief position PID regulator
 **
 ** \param [in] float Ref:  reference
 **             float Fb:   feedback
 **             stc_pos_pid_t* pstc: pointer to PID regulator
 **
 ** \retval     float Output
 ******************************************************************************/

float PID_PosPid(float Ref, float Fb, stc_pos_pid_t *pstc) {
    float Err = Ref - Fb;
    /** limit maximum error ***************************************************/
    if (Err > pstc->AbsErrMax) {
        Err = pstc->AbsErrMax;
    } else if (Err < -pstc->AbsErrMax) {
        Err = -pstc->AbsErrMax;
    }

    float dErr = Err - pstc->Err1;  // 计算误差的微分

    float Pout = pstc->Kp * Err;  // 比例项输出
    float Dout = pstc->Kd * dErr;  // 微分项输出

    float tempIout = pstc->Iout;
    // 积分分离算法
    if (fabsf(Err) <= pstc->ErrSeparation) {
        tempIout += pstc->Ki * Err;  // 误差小于阈值时，进行积分计算
    } else {
        tempIout = 0;  // 误差大于阈值时，积分项清零
    }

    // 抗积分饱和算法
    float tempOutput = Pout + tempIout + Dout;
    if (tempOutput > pstc->OutMax) {
        if (Err > 0) {
            // 当输出超过最大值且误差为正，不增加积分项
            tempIout = pstc->Iout;
        }
    } else if (tempOutput < pstc->OutMin) {
        if (Err < 0) {
            // 当输出小于最小值且误差为负，不增加积分项
            tempIout = pstc->Iout;
        }
    }

    pstc->Iout = tempIout;

    // 积分项输出限制
    if (pstc->Iout > pstc->OutMax) {
        pstc->Iout = pstc->OutMax;
    } else if (pstc->Iout < pstc->OutMin) {
        pstc->Iout = pstc->OutMin;
    }

    /** sum of P, I and D regulator **********************************************/
    pstc->Output = Pout + pstc->Iout + Dout;

    /** output limitation *****************************************************/
    if (pstc->Output > pstc->OutMax) {
        pstc->Output = pstc->OutMax;
    } else if (pstc->Output < pstc->OutMin) {
        pstc->Output = pstc->OutMin;
    }

    pstc->Err2 = pstc->Err1;
    pstc->Err1 = Err;
    return pstc->Output;
}
void PID_InitMITPid(float Kp,float Kd, stc_mit_pid_t *pstc)
{
    pstc->pos_Kp = Kp;
    pstc->spd_Kd = Kd;
    pstc->Output = 0;
}

float PID_MITPid(float Pos_Ref, float Pos_Obser,float Spd_Ref,float Spd_Obser, float Torque_Ref,stc_mit_pid_t *pstc)
{
    pstc->Output = pstc->pos_Kp*(Pos_Ref-Pos_Obser)+pstc->spd_Kd*(Spd_Ref-Spd_Obser)+Torque_Ref;
    return pstc->Output/(float)(Ke*MOTOR_ReductionRatio);
}
